{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "0824e695-9bc7-4b3e-8828-6021b2ff0a5f",
   "metadata": {},
   "source": [
    "# Driving in Straight Line and maintain speed"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "bd422d6a-2f28-43ab-82ad-52913757f9e1",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Hello there\n"
     ]
    }
   ],
   "source": [
    "print(\"Hello there\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "5515777e-5345-4d1c-a17c-1ef1370c781f",
   "metadata": {},
   "outputs": [],
   "source": [
    "import carla\n",
    "import time\n",
    "import numpy as np\n",
    "import math\n",
    "import sys"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "397c6ee2-16d3-45a3-9659-291dd7631081",
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "331aa792-ee0f-4a66-b039-6e1a7bfb48c3",
   "metadata": {},
   "outputs": [],
   "source": [
    "#connect to simulator\n",
    "client = carla.Client('localhost', 2000)\n",
    "world = client.get_world()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "de8b2ed2-7470-48e2-8e34-f20b53d61149",
   "metadata": {},
   "outputs": [],
   "source": [
    "#utility script for destruction, clear all the actors (basically undo or destroy the vehicle)\n",
    "for actor in world.get_actors().filter('*vehicle*'):\n",
    "    actor.destroy()\n",
    "#remove top and bottom qoutes to execute"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "ff3d687c-c414-4956-8471-2dee2e5cbd0f",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'\\n#get location of the client (I use this to confirm the connection with the sim)\\nspectator = world.get_spectator()\\ntransform = spectator.get_transform()\\nspectator.set_transform(carla.Transform())\\n'"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "'''\n",
    "#get location of the client (I use this to confirm the connection with the sim)\n",
    "spectator = world.get_spectator()\n",
    "transform = spectator.get_transform()\n",
    "spectator.set_transform(carla.Transform())\n",
    "'''"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "fa83c97e-4f63-4f65-abe6-33f69b62893b",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "\"\\n#print all the vehicles\\nfor bp in world.get_blueprint_library().filter('vehicle'):\\n    print(bp.id)\\n\\nvehicle.tesla.model3\\n\""
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "'''\n",
    "#print all the vehicles\n",
    "for bp in world.get_blueprint_library().filter('vehicle'):\n",
    "    print(bp.id)\n",
    "\n",
    "vehicle.tesla.model3\n",
    "'''"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "a9ee442d-382f-4f9a-8649-7b46e7dd1030",
   "metadata": {},
   "outputs": [],
   "source": [
    "# get the spawn points of world\n",
    "spawn_points = world.get_map().get_spawn_points()\n",
    "start_point = spawn_points[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "bac98504-d10f-4b90-a6e9-bdfa573e911d",
   "metadata": {},
   "outputs": [],
   "source": [
    "#look for a blue print of your desired car, i want tesla\n",
    "vehicle_bp = world.get_blueprint_library().filter('vehicle.tesla.model3')\n",
    "#print(vehicle_bp)\n",
    "\n",
    "#spawn the vehicle on start point\n",
    "vehicle = world.try_spawn_actor(vehicle_bp[0], start_point)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "f9ec4760-9b35-4a1d-85ab-6c9baf103fd6",
   "metadata": {},
   "outputs": [],
   "source": [
    "# position and create camera and open a live stream of car\n",
    "CAMERA_POS_Z = 3\n",
    "CAMERA_POS_X = -5\n",
    "\n",
    "camera_bp = world.get_blueprint_library().find('sensor.camera.rgb')\n",
    "camera_bp.set_attribute('image_size_x', '1920')\n",
    "camera_bp.set_attribute('image_size_y', '1080')\n",
    "\n",
    "camera_init_trans = carla.Transform(carla.Location(z=CAMERA_POS_Z, x=CAMERA_POS_X))\n",
    "camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to = vehicle)\n",
    "\n",
    "def camera_callback(image, data_dict):\n",
    "    data_dict['image'] = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))\n",
    "\n",
    "image_w = camera_bp.get_attribute('image_size_x').as_int()\n",
    "image_h = camera_bp.get_attribute('image_size_y').as_int()\n",
    "\n",
    "camera_data = {\n",
    "    'image': np.zeros((image_h, image_w, 4))\n",
    "}\n",
    "\n",
    "camera.listen(lambda image: camera_callback(image, camera_data))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "98e9725a-5c3a-464f-b15c-d07c5c8d0d66",
   "metadata": {},
   "outputs": [],
   "source": [
    "# define constants \n",
    "\n",
    "#define speed constants\n",
    "PREFERRED_SPEED = 30\n",
    "SPEED_TRESHOLD = 2\n",
    "\n",
    "#adding parameters to display text to the image\n",
    "font = cv2.FONT_HERSHEY_SIMPLEX\n",
    "\n",
    "#defining the lines and font to display\n",
    "org = (30, 30)\n",
    "org2 = (30, 50) # this line will be used for future steering angle\n",
    "fontScale = 0.5\n",
    "color = (255, 255, 255)\n",
    "thickness = 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "b4b29ce4-ad06-4755-8976-6464d8989f02",
   "metadata": {},
   "outputs": [],
   "source": [
    "# function to miantain the speed\n",
    "def maintain_speed(s):\n",
    "    if s >= PREFERRED_SPEED:\n",
    "        return 0\n",
    "    elif s < PREFERRED_SPEED - SPEED_TRESHOLD:\n",
    "        return 0.8\n",
    "    else:\n",
    "        return 0.4"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "f7fd2203-1afb-49d2-a4e0-121a8ae3f635",
   "metadata": {},
   "outputs": [],
   "source": [
    "# drive straight and maintain the speed\n",
    "\n",
    "cv2.namedWindow('RGB Camera', cv2.WINDOW_AUTOSIZE)\n",
    "cv2.imshow('RGB Camera', camera_data['image'])\n",
    "\n",
    "quit = False\n",
    "\n",
    "while True:\n",
    "    world.tick()\n",
    "    if cv2.waitKey(1) == ord('q'):\n",
    "        quit - True\n",
    "        break\n",
    "    image = camera_data['image']\n",
    "    steering_angle = 0\n",
    "    v = vehicle.get_velocity()\n",
    "    speed = round(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2),0)\n",
    "    \n",
    "    #display speed on window\n",
    "    image = cv2.putText(image, \n",
    "                        'Speed: '+str(int(speed))+' kmh',\n",
    "                        org2, font, fontScale, color, thickness, cv2.LINE_AA)\n",
    "\n",
    "    # determine the input from current speed\n",
    "    estimated_throttle = maintain_speed(speed)\n",
    "    #now accelrate\n",
    "    vehicle.apply_control(carla.VehicleControl(throttle=estimated_throttle, steer = steering_angle))\n",
    "    cv2.imshow('RGB Camera', image)\n",
    "\n",
    "#clean up\n",
    "cv2.destroyAllWindows()\n",
    "camera.stop()\n",
    "for actor in world.get_actors().filter('*vehicle*'):\n",
    "    actor.destroy()\n",
    "for sensor in world.get_actors().filter('*sensor*'):\n",
    "    sensor.destroy()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9223166a-0c6e-41ca-8ded-ed8ad5c4a99d",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b82b8068-900b-4eb3-aa60-092b16a69dce",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
